#include <Wire.h>
#include <EasyTransferI2C.h>
#include <AFMotor.h>
#include "Motor.h"

//create I2C Transfer Object 
EasyTransferI2C ET; 

//this is ths command structure for sending and receiving data between the master controller and nodes
struct RECEIVE_DATA_STRUCTURE{
  //put your variable definitions here for the data you want to send
  //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
  char command; //command to execute on the node
  int data;    //data needed to execute command, if any.
};
//give a name to the group of data
RECEIVE_DATA_STRUCTURE i2cdata;

//define slave i2c address
#define I2C_SLAVE_ADDRESS  9   //address of the motor controller board
#define I2C_MASTER_ADDRESS 1   //address of the master controller


//led pin on Arduino
int ledPin = 13;
//Create 4 dc motor  objects - one for each wheel
// make the pwm 1khz to conserve power
// NOTE: only motors 1 ans 2 can be run at higher rates than 1 KHz, 3 and 4, default to 1 KHz
AF_DCMotor motor1(1, MOTOR12_1KHZ); // create motor #1, 1KHz pwm
AF_DCMotor motor2(2, MOTOR12_1KHZ); // create motor #2, 1KHz pwm
AF_DCMotor motor3(3, MOTOR34_1KHZ); // create motor #3, 1KHz pwm
AF_DCMotor motor4(4, MOTOR34_1KHZ); // create motor #4, 1KHz pwm


void setup()
{
	//init ledpin as output
	pinMode(ledPin, OUTPUT);
	//just some motor moves to see if everything is fucntioniong
	TurnLeft(128);
	delay(500);
	TurnRight(128);
	delay(500);
	Forward(128);
	delay(500);
	Reverse(128);
	delay(500);
	Stop();

  Wire.begin(I2C_SLAVE_ADDRESS);  //start the library, pass in the data details and the name of the serial port. Can be Serial, Serial1, Serial2, etc.
  ET.begin(details(i2cdata), &Wire);

  //define handler function on receiving data
  Wire.onReceive(I2Creceive);

}

void loop()
{
  /* add main program code here */
  digitalWrite(13, HIGH);   // set the LED on
  delay(250);              // wait for a second
  digitalWrite(13, LOW);    // set the LED off
  delay(250);              // wait for a s
  //	motor1.run(BACKWARD);      // turn it on going forward
 // RunMotorBackwards(3000);

	if(ET.receiveData())
	{

		switch(i2cdata.command)
		{
				case(0x01): //command 0x01 - forward
					Forward(i2cdata.data);
				 break;

				case(0x02): //command 0x02 - Reverse
					Reverse(i2cdata.data);
				 break;

				case(0x03): //command 0x03 - Turn left
					TurnLeft(i2cdata.data);
				 break;

				case(0x04): //command 0x04 - Turn Right
					TurnRight(i2cdata.data);
				break;

				case(0x05): //command 0x05 - Turn Right
					Stop();
				 break;
		}

	}


}


void I2Creceive(int numbytes)
{
}